#ifndef URCONTROL_HPP
#define URCONTROL_HPP

#include <thread>
#include <chrono>

#include "urscript.hpp"

#define INLINUX

#ifdef INLINUX
#include "urcontrollinux.hpp"
#else
#include "urcontrolqt.hpp"
#endif


#include "ToArduinoA.hpp"


class URControl
        : public URScript,
        #ifdef INLINUX
        public URControlLinux
        #else
        public URControlQT
        #endif
{

public:
    static void DashBoardCommand(const char *ipUR, const char *command, uint16_t dashboardPort=29999)
    {
        startclient(dashboardPort, ipUR, command);
    }

    template<typename T>
    static void SetTCPPose(const char *ipUR, T x, T y, T z, T rx, T ry, T rz, T a=0.02, T v=0.1, uint16_t portUR=30003)
    {
        std::string cmd=SetTCPPoseScript(x,y,z,rx,ry,rz,a,v);
        startclient(portUR, ipUR, cmd.c_str());
    }

    static std::string ScriptCommand(std::string script, const char *ipUR, const char *ipPC, uint16_t portUR=30003, uint16_t portPC=1235)
    {
        std::string cmd=GetTCPPoseScript(ipPC,portPC,script);
        std::string result;
        std::thread th(startserver, portPC, std::ref(result));
        startclient(portUR, ipUR, cmd.c_str());
        if(th.joinable()) th.join();
        return result;
    }

    template<typename T>
    static void SetTCPPose(const char *ipUR, const char *ipPC, T x, T y, T z, T rx, T ry, T rz, T a=0.02, T v=0.1, char f='l', T r=0)
    {
        std::string result=ScriptCommand(SetTCPPoseScript(x,y,z,rx,ry,rz,a,v,f,r),ipUR,ipPC);
    }

//    0<=openmm<=101
    template<typename T>
    static void RG2(const char *ipUR, const char *ipPC, T openmm)
    {
        if(openmm<0) openmm=0;
        if(openmm>101) openmm=101;
        std::string result=ScriptCommand(RG2Script(openmm),ipUR,ipPC);
    }

    template<typename T>
    static void RG6(const char *ipUR, const char *ipPC, T openmm)
    {
        std::string result=ScriptCommand(RG6Script(openmm),ipUR,ipPC);
    }

    template<typename T>
    static void GetTCPPose(const char *ipUR, const char *ipPC, T &x, T &y, T &z, T &rx, T &ry, T &rz)
    {
        std::string result=ScriptCommand("",ipUR,ipPC);
        DecodeTCPResult(result, x, y, z, rx, ry, rz);
    }

    static void ResetTCP(const char *ipUR, const char *ipPC)
    {
        std::string result=ScriptCommand(ResetTCPScript(),ipUR,ipPC);
    }

    static void LoadAndPlay(const char *ipUR, std::string urpfile, uint16_t portUR=29999, uint16_t portPC=1235)
    {
        std::string cmd="load /programs/"+urpfile+".urp";//the script should contain code for response when it ends
        std::string result;
        std::thread th(startserver, portPC, std::ref(result));
        startclient(portUR, ipUR, cmd.c_str());
        cmd="play";
        startclient(portUR, ipUR, cmd.c_str());
        if(th.joinable()) th.join();
    }

    template<typename T>
    static void GetTCPPoseModbus(const char *ipUR, T &x, T &y, T &z, T &rx, T &ry, T &rz, uint16_t modbusPort=502)
    {
        short pos[6];
        ReadModbusRegister(ipUR,6,400,pos,modbusPort);
        x=pos[0]/10000.0;
        y=pos[1]/10000.0;
        z=pos[2]/10000.0;
        rx=pos[3]/1000.0;
        ry=pos[4]/1000.0;
        rz=pos[5]/1000.0;
    }

    static void test()
    {
        char ipUR[]="192.168.0.200";
        char ipPC[]="192.168.0.102";
#if 1
        double p[6];
        GetTCPPose(ipUR,ipPC, p[0],p[1],p[2],p[3],p[4],p[5]);
        printf("[%g,%g,%g,%g,%g,%g]\n",p[0],p[1],p[2],p[3],p[4],p[5]);

        GetTCPPoseModbus(ipUR,p[0],p[1],p[2],p[3],p[4],p[5]);
        printf("[%g,%g,%g,%g,%g,%g]\n",p[0],p[1],p[2],p[3],p[4],p[5]);
#endif
        //                RG6(ipUR,ipPC, 80);

//        RG2(ipUR,ipPC, 0);
//        RG2(ipUR,ipPC, 102);

    }


    static void UR5Demo()
    {
        char ipUR[]="192.168.1.13";
        char ipPC[]="192.168.1.14";

        ToST tst;

        tst.StartUpdate();

        double step=2;

        for(int i=0;i<50;i++)
        {

            for(double i=100;i>10;i-=step)
            {
                RG6(ipUR, ipPC, i);
                std::cout<<i<<"\n"<<std::flush;


                while(tst.IsStop())
                {
                    std::this_thread::yield();

                }

            }


            for(double i=10;i<100;i+=step)
            {
                RG6(ipUR, ipPC, i);
                std::cout<<i<<"\n"<<std::flush;
                while(tst.IsStop())
                {

                    std::this_thread::yield();
                }
            }


        }
        tst.EndUpdate();

    }
};

#endif // URCONTROL_HPP
